#ifndef RSIM_FASTOPS_H
#define RSIM_FASTOPS_H

/* -------------------------------------------------------------------------- *
* File: FastOps.h							      *
* Authors: Kishor Bhalerao                                                   *
* Email : kishor8dm@gmail.com                                                *
* Contributors:                      					      *
* Email: 								      *
* copyright (c) 2010 Authors.                                                *
*                                                                            *
* This program is free software: you can redistribute it and/or modify it    *
* under the terms of the GNU General Public License as published by the Free *
* Software Foundation, either version 3 of the License, or any later version.*
*									      *
* This program is distributed in the hope that it will be useful, but        *
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
* or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
* for more details. (http://www.gnu.org/licenses/)			      *
* -------------------------------------------------------------------------- */

#include "LinAlgDecl.h"

#include <iostream>
#include <cstdlib>
#include <cmath>
#include <assert.h>

using namespace std;
namespace RSIM{

// Forward declaration for class List
template <class T> class List;
        
namespace FastOps{
        // The functions of namespace FastOps are generated using the command ccode in Matlab 
        // Refer http://www.mathworks.com/help/toolbox/symbolic/ccode.html for more details

        /** SM = R_AB * M * transpose(R_AB), where R_AB is a 3x3 rotation matrix between frames B and A.
        M is a 6x6 matrix expressed in frame B. SM is a 6x6 matrix expressed in frame A
        */
        void spatial_basis_shift_mat(const double *c, const double *m, double *sm);
                
        /** SV = transpose(R_AB) * V, where R_AB is a 3x3 rotation matrix between frames B and A.
        V is a 6x1 spatial vector expressed in frame A. SV is a 6x1 vector expresed in frame B.
        */
        void transpose_spatial_basis_shift_vec(const double *c, const double *v, double *sv);
                
        /** SM = R_AB * M * transpose(R_AB). Converts 3x3 matrix M from frame B to frame A */
        void basis_shift_mat33(const double *c, const double *m, double *sm);
                
        /** SM = transpose(R_AB) * M * R_AB. Converts 3x3 matrix M from frame A to frame B */
        void transpose_basis_shift_mat33(const double *c, const double *m, double *sm);
                
        /** SV = transpose(R_AB) * V, Converts 3x1 vector V from frame A to frame B */
        void transpose_basis_shift_vec3(const double *c, const double *v, double *sv);
                
        /** SV = C * V */
        void spatial_basis_shift_vec(const double *c, const double *v, double *sv);
                
        /** if skbeta = [I betax; 0 I], then SV = skbeta*V*/
        void spatial_shift_vec6(const double *V, const double *beta, double *SV);
                
        /** if skbeta = [I betax; 0 I] and V = [0 0 0 F0 F1 F2]', then SV =skbeta*V */
        void spatial_shift_vec3(const double *F, const double *beta, double *SV);
                
        /** 
        SK = [U Rx;0 U], where 0 and U are 3x3 zero and identity matrices, respectivey. Then, SV = Sk * V
        R and V are expected to be given in the same basis.
        */
        void shift_spatial_vec(const double *R, const double *V, double *sv);
                
        /** invM = inverse(M) */
        void inv_mat3x3(const double *M, double *invM);
                
        /** wwr = cross(w,cross(w,r)) */
        void wxwxr(const double *w, const double *r, double *wwr);
                
        /** v3 = cross(v1,v2) */
        void cross(const double *v1, const double *v2, double *v3);
                
        /** M3 = M1*M2 */
        void mult_mat33_mat33(const double *m1, const double *m2, double *m3);
                
        /** OP = M*V */
        void mult_mat66_vec6(const double *m, const double *v, double *op);
                
        /** 3x3 Matrix m = [Vx].*/
        void formCrossProdMat(const double *v, double *m);
                
        /** V2 = M*V1 */
        void mult_mat33_vec3(const double *m, const double *v1, double *v2);
                
        /** op = transpose(M)*V, where V = {v0,v1,v2} */
        void transpose_mult_mat33_vec3(const double *m, const double& v0,const double& v1,const double& v2, double *op);
                
        /** op = transpose(M)*V */
        void transpose_mult_mat33_vec3(const double *m, const double *v, double *op);
                
        /** if skbeta = [I betax; 0 I] and V = [F0 F1 F2 F3 F4 F5]', then SV =transpose(skbeta)*V */
        void transpose_skbeta_x_V(const double *V, const double *beta, double *SV);
        
        inline void normalizeVector(double *v, const int& num_ele){
                double norm = 0.0;
                for(int i=0;i<num_ele;++i)
                        norm += v[i]*v[i];
                norm = sqrt(norm);
                assert(norm);
                for(int i=0;i<num_ele;v[i++] /= norm);
                        
        }
        
} // namespace FastOps


} // namespace RSIM

#endif